/** ###################################################################
**     Filename  : ParkControl.c
**     Project   : parking
**     Processor : MC9S12XS128MAL
**     Compiler  : CodeWarrior HCS12X C Compiler
**     Date/Time : 2013-3-9, 15:08
**     Contents  :
**         User source code
**
** ###################################################################*/

/* MODULE ParkControl */

#include "ParkControl.h"
#include "CommandSystem.h"
#include "Hmi.h"
#include "Drv.h"
#include "EM.h"
#include "MEMORY.h"
#include "Test.h"
#include "stdio.h"
#include "Communication.h"
#include "Lights.h"

//for jump
U8 PCInitProc = 0;

//for stop gently
bool PCWillStop = FALSE;
S16 PCCurrentStopDistance = PC_DEFAULT_STOP_DISTANCE;
S16 PCCurrentStopSpeed	= PC_DEFAULT_STOP_SPEED;

//
volatile U8 PCCurrentProc;
S16 PCLastProcDist;
S16 PCCurrentProcDist;
S16 PCCurrentProcTargetDist;
S16 PCTimerCountDown;
//for distance
U16 PCpPreviousAccumulator;
S32 PCpCurrentDistance;
ParkProcedure PCProcedure[CS_MAX_COMMAND + 1]; 
bool PCIsRunning = FALSE;
bool PCTimerFrozen = FALSE;
bool PCInStop = FALSE;
bool PCInMileStone	= FALSE;
bool PCInLoop = FALSE;
S16 PCTrigState	= PC_TRIG_INIT;
S16 PCTrigFarStartPos = 0;  //don't need to initialize this
						  
static S16 PCpGetCurrentDistance(void)
{
	return (S16)(PCpCurrentDistance / PC_DISTANCE_SCALE);
}

static void PCpMeasureDistance(void)
{
	U16 data;
	volatile S16 delta;
	HmiQueryPulseAccumulator(&data);
	delta = (S16)(data - PCpPreviousAccumulator );
	PCpCurrentDistance += delta;
	PCpPreviousAccumulator = data;	
}

static void PCpStartControl(void){
	DrvEnableMotorControl();
	(void)DRV_SERVO_Enable();
	DRV_SERVO_SetDuty(DrvServoMiddle);	
}

static void PCpStopControl(void){
	DrvDisableMotorControl();
	(void)DRV_SERVO_Disable();	
}

static void PCLoadProc(U8 proc, S16 current_dist, S16 id){
	char str[30];
	U16 len;
	Cpu_Delay100US(20);
	(void)sprintf(str, "proc:%u\tdist = %d\tid = %d\t\n"
		, proc, current_dist, id);
	len = strlen(str);
	ASSendData((U8*)&str[0], len);
	if (PCCurrentProc >= CS_MAX_COMMAND){
		PCExit();
	}else{
		PCLastProcDist = current_dist;
		PCCurrentProcDist = 0;
		PCCurrentProcTargetDist = PCProcedure[proc].distance;
		PCTrigState = PC_TRIG_INIT;	
	}
	if (PCProcedure[proc + 1].mode == CS_STOP 
		|| PCProcedure[proc + 1].mode == CS_MILESTONE){
		PCWillStop = TRUE;
		PCCurrentStopDistance = (PCCurrentProcTargetDist > PC_DEFAULT_STOP_DISTANCE)?PC_DEFAULT_STOP_DISTANCE : 0;
		PCCurrentStopSpeed	= (PCProcedure[proc].speed > PC_DEFAULT_STOP_SPEED) ? PC_DEFAULT_STOP_SPEED : PCProcedure[proc].speed;
	}else{
		PCWillStop = FALSE;	
		PCCurrentStopDistance = PC_DEFAULT_STOP_DISTANCE;
		PCCurrentStopSpeed	= PC_DEFAULT_STOP_SPEED;
	}
	LTAllOff();
}



    
void PCSetDir(S16 dir){	
	U16 duty = (U16)((S16)DrvServoMiddle + dir);
	DRV_SERVO_SetDuty(duty);	
}

void PCSetSpeed(S16 speed){
	DrvSetSpeedMotor(speed);		
}




void PCPrepare(void){
    PCIsRunning = TRUE;
  	PCTimerFrozen = FALSE;
  	PCInStop = FALSE;
  	PCInMileStone=FALSE;
  	(void)PARK_TIMER_Disable();
  	HmiQueryPulseAccumulator(&PCpPreviousAccumulator);
  	PCpCurrentDistance = 0;
  	PCCurrentProc = PCInitProc;
  	PCInitProc = 0;
  	PCCurrentProcDist = 0;
  	PCLastProcDist = 0;
  	PCCurrentProcTargetDist = 0;
  	PCWillStop = FALSE;
	PCCurrentStopDistance = PC_DEFAULT_STOP_DISTANCE;
	PCCurrentStopSpeed	= PC_DEFAULT_STOP_SPEED;
  	PCLoadProc(PCCurrentProc, PCpGetCurrentDistance(), 0);
  	PCpStartControl();
    return;
}



void PCExecProc(U8 proc){		
	switch(PCProcedure[proc].mode){
		case CS_FORWARD:{
			PCSetDir(PCProcedure[proc].direction);
		//	PCSetSpeed(PCProcedure[proc].speed);
			LTLightUponDir(PCProcedure[proc].direction);
			LTStopOff();			
		}
			break;
		case CS_BACKWARD:{
			PCSetDir(PCProcedure[proc].direction);
		//    PCSetSpeed(- PCProcedure[proc].speed);
			LTLightUponDir(PCProcedure[proc].direction);	
			LTStopOff();			
		}
			break;
		case CS_AUTO_FORWARD:{
			EMDriveForward();
		//	PCSetSpeed(PCProcedure[proc].speed);
			LTStopOff();	
		}
			break;
		case CS_AUTO_BACKWARD:{
			EMDriveBackward();
		//	PCSetSpeed(- PCProcedure[proc].speed);	
			LTStopOff();
		}
			break;
		case CS_STOP:{
		    if (!PCInStop && proc == PCCurrentProc){
				PCInStop = TRUE;
				PCSetDir(PCProcedure[proc].direction);
				PCSetSpeed(0);
				PCTimerCountDown = (S16)PCProcedure[proc].distance * PC_TIMER_SCALE;
				(void)PARK_TIMER_Enable();	
		    }
		    LTStopOn();
		}
			break;
		case CS_MILESTONE:{
			if (!PCInLoop){
				if (PCInStop){
					(void)PARK_TIMER_Disable();	
					PCInStop = FALSE;	   //may be interrupted, don't know the side effect
				}
				if (!PCInMileStone && proc == PCCurrentProc){
					PCInMileStone = TRUE;
					PCSetDir(PCProcedure[proc].direction);
					PCSetSpeed(0);	
				}
			}else{
				PCInMileStone = FALSE;
				if (!PCInStop && proc == PCCurrentProc){
					PCInStop = TRUE;
					PCSetDir(PCProcedure[proc].direction);
					PCSetSpeed(0);
					PCTimerCountDown = (S16)PCProcedure[proc].distance * PC_TIMER_SCALE;
					(void)PARK_TIMER_Enable();
				}
			}
			LTStopOn();
		}
			break;
		case CS_EMPTY:
		default:
		{
			PCExit();
		}
			break;
	}
}

//parking control;
void PCDrive(void){
	
		char str[100];
		U16 len;
		
	while(PCIsRunning){
		S16 dist;
		
		PCExecProc(PCCurrentProc);
		PCpMeasureDistance();
		dist = PCpGetCurrentDistance();
		//for debug
		HmiShowNumber((U16)PCCurrentProc + 1);
		switch (PCProcedure[PCCurrentProc].mode){
			case CS_STOP: 
			case CS_MILESTONE:
			{
				continue;
			}
				break;
			case CS_BACKWARD:
			case CS_AUTO_BACKWARD:
			{
				PCCurrentProcDist = PCLastProcDist - dist;
				if (PCWillStop 
					&& PCCurrentProcDist < PCCurrentProcTargetDist 
					&& PCCurrentProcTargetDist - PCCurrentProcDist < PCCurrentStopDistance){
					S16 speed = PCCurrentStopSpeed;
					PCSetSpeed(-speed);
				}else{
					PCSetSpeed(- PCProcedure[PCCurrentProc].speed);		
				}
			}
				break;
			case CS_FORWARD:
			case CS_AUTO_FORWARD:
			{
				PCCurrentProcDist = dist - PCLastProcDist;
				if (PCWillStop 
					&& PCCurrentProcDist < PCCurrentProcTargetDist 
					&& PCCurrentProcTargetDist - PCCurrentProcDist < PCCurrentStopDistance){
					S16 speed = PCCurrentStopSpeed;
					PCSetSpeed(speed);
				}else{
					PCSetSpeed(PCProcedure[PCCurrentProc].speed);		
				}
			}
				break;
			default:
				break;
		}
		
		if (PCCurrentProcDist >= PCCurrentProcTargetDist){
			goto NEXT;	
		}
		
		if (PCProcedure[PCCurrentProc].trig_min > 0 
				&& PCProcedure[PCCurrentProc].extended <= 0
				&& PCInfDetect()
				&& PCCurrentProcDist >= PCProcedure[PCCurrentProc].trig_min){
			goto NEXT;	
		}
		
		if (PCProcedure[PCCurrentProc].trig_min > 0 
				&& PCProcedure[PCCurrentProc].extended > 0){
			if (PCTrigState == PC_TRIG_INIT){
				if (!PCInfDetect()){
					PCTrigState = PC_TRIG_WAIT_NEAR;	
					PCTrigFarStartPos = PCCurrentProcDist; 
				}
			}else if(PCTrigState == PC_TRIG_WAIT_NEAR){
				if (PCInfDetect()){
					if ((PCCurrentProcDist - PCTrigFarStartPos) >= PCProcedure[PCCurrentProc].extended){
						if (PCCurrentProcDist >= PCProcedure[PCCurrentProc].trig_min){
							goto NEXT;	
						}
					}else{
						PCTrigState = PC_TRIG_INIT;		
					}
				}
			}	
		//	goto NEXT;	
		}
		continue;
NEXT:
		(void)sprintf(str, "proc:%u\tcurrent = %d\ttarget = %d\n", PCCurrentProc, PCCurrentProcDist, PCCurrentProcTargetDist);
		len = strlen(str);
		ASSendData((U8*)&str[0], len);
		++PCCurrentProc;
		PCLoadProc(PCCurrentProc, dist, 1);
					
	}
}

void PCTimerFreeze(void){
	PCTimerFrozen = TRUE;		
}

void PCTimerRestore(void){
	PCTimerFrozen = FALSE;		
}

void PCTimerIrqHandler(void){
	if (!PCTimerFrozen){
		--PCTimerCountDown;
		if (PCTimerCountDown <=0){
			(void)PARK_TIMER_Disable();
			PCpMeasureDistance();		   
			++PCCurrentProc;
			PCLoadProc(PCCurrentProc, PCpGetCurrentDistance(), 2);
			PCInStop = FALSE;
		}
	}
}

void PCExit(void){
	PCpStopControl();
	PCIsRunning = FALSE;
	PCInitProc = 0;
	LTAllOff();
		
}

bool PCInfDetect(void){
	return !INF_GetVal();	
}

void PCGetFlagMemory(U16 offset, U16 *flag){
	unsigned int *far memory_ptr = (unsigned int *far)PC_FLAG_MEMORY_ADDR + offset;
	while(ERR_OK != MEMORY_GetWord(memory_ptr, (unsigned int *)flag));		
}



void PCSetFlagMemory(U16 offset, U16 flag){
	unsigned int *far memory_ptr = (unsigned int *far)PC_FLAG_MEMORY_ADDR + offset;	
	while(ERR_OK != MEMORY_SetWord(memory_ptr, (unsigned int)flag));	
	
}

void PCStoreProcedure(void){   
	unsigned int *far memory_ptr = (unsigned int *far)PC_PROCEDURE_MEMORY_ADDR;
	S16 *procedure_ptr = (S16*)&PCProcedure;
	U16 i;
	for (i = 0; i < CS_MAX_COMMAND * sizeof(ParkProcedure) / sizeof(S16); ++i){
		HmiShowNumberPoint(i);
		while(ERR_OK != MEMORY_SetWord(memory_ptr + i, (unsigned int)*(procedure_ptr + i)));
	}
}


     

void PCLoadProcedure(void){
	unsigned int *far memory_ptr = (unsigned int *far)PC_PROCEDURE_MEMORY_ADDR;
	S16 *procedure_ptr = (S16*)&PCProcedure;
	U16 i;
	for (i = 0; i < CS_MAX_COMMAND * sizeof(ParkProcedure) / sizeof(S16); ++i){
		HmiShowNumberPoint(i);
		while(ERR_OK != MEMORY_GetWord(memory_ptr + i, (unsigned int*) (procedure_ptr + i)));
	}
}

void PCPassMileStone(void){
	if (PCInMileStone){
		PCpMeasureDistance();
		++PCCurrentProc;
		PCLoadProc(PCCurrentProc, PCpGetCurrentDistance(), 3);
		PCInMileStone = FALSE;
	}
}

void PCJump(S16 seq){
	PCExit();	
 	PCInitProc = (U8)seq;
}

void PCInit(void){
	U16 pc_flag;
	PCGetFlagMemory(PC_FLAG_PROCEDURE_OFFSET, &pc_flag);
	PCInitProc = 0;
	if (pc_flag != PC_FLAG_MAGIC){
		TstInitParkControl();//not good to use test	
	}else{
		PCLoadProcedure();	
	}
}


void PCGetRunningStatus(bool *status, bool *mode){
	*status = !PCTimerFrozen;
	*mode = PCInLoop;	
}
void PCGetStatus(S16 *seq, S16 *distance){
	*seq = (S16)PCCurrentProc;
	if (PCProcedure[PCCurrentProc].mode == CS_STOP){
		*distance = PCTimerCountDown / PC_TIMER_SCALE;	
	}else{
		*distance = PCCurrentProcDist;	
	}
}

void PCLoop(S16 state){
	if (state != 0){
		PCInLoop = TRUE;	
	}else{
		PCInLoop = FALSE;	
	}	
}





/* END ParkControl */
